package algorithms.astar;

import java.awt.Point;
import java.util.ArrayDeque;
import java.util.Deque;
import java.util.HashMap;
import java.util.Iterator;
import java.util.PriorityQueue;

import pathfinding.gui.Map;
import algorithms.DescriptedPoint;
import algorithms.PathFindingAlgorithm;
import algorithms.SearchResult;
import algorithms.astar.heuristics.HCostHeuristic;

/**
 * Class for A* path finding algorithm implementation. For more information
 * about A*, see<br>
 * http://en.wikipedia.org/wiki/A*_search_algorithm<br>
 * http://www.policyalmanac.org/games/aStarTutorial.htm<br>
 * 
 * @author teturun
 */
public class AStarAlgorithm implements PathFindingAlgorithm {

	private AStarCost aStarCost;

	/**
	 * Constructor for AStarAlgorithm.
	 */
	public AStarAlgorithm() {
		this.aStarCost = new AStarCost();
	}

	/**
	 * Returns cost heuristics that can be used in this A* implementation.
	 * 
	 * @return AStarCost object containing heuristics
	 */
	public AStarCost getCostHeuristics() {
		return this.aStarCost;
	}

	/**
	 * Sets h cost heuristic (see some A* tutorial for more information) for
	 * this A* implementation
	 * 
	 * @param heuristic
	 *            h cost heuristic
	 */
	public void setHCostHeuristic(HCostHeuristic heuristic) {
		this.aStarCost.setHCostHeuristic(heuristic);
	}

	@Override
	/**
	 * Overrides and implements search for A*
	 */
	public SearchResult findPath(Point startPoint, Point goalPoint, Map map) {

		if (startPoint == null || goalPoint == null || map == null)
			return null;

		System.out.println("A* Search begins...\nheuristic: "
				+ this.aStarCost.getHCostHeuristic());

		Deque<DescriptedPoint> path = new ArrayDeque<DescriptedPoint>();
		PriorityQueue<Node> open = new PriorityQueue<Node>();
		HashMap<DescriptedPoint, Node> closed = new HashMap<DescriptedPoint, Node>();

		DescriptedPoint start = new DescriptedPoint(startPoint.x, startPoint.y);
		DescriptedPoint goal = new DescriptedPoint(goalPoint.x, goalPoint.y);
		
		long timeBeforeSearch = System.nanoTime();

		this.doSearch(start, goal, map, open, closed, path);

		long searchTime = System.nanoTime() - timeBeforeSearch;
		long searchTimeInMillis = searchTime / 1000000;
		
		SearchResult searchResult = null;

		for (DescriptedPoint point : path) {
			point.setDescription("<html>Path cell" +
								 "<br>Path length in steps: " + path.size() +
								 "<br>Cells in path: " + (path.size()-1) +
								 "<br>Search time: " + searchTimeInMillis + " ms");
		}
		
		Deque<DescriptedPoint> usedCandidates = new ArrayDeque<DescriptedPoint>();
		for (Node n : closed.values()) {
			DescriptedPoint point = n.getLocation();
			point.setDescription("<html>Visited and selected cell" +
								"<br>A* G cost: " +  n.getGCost() +
								"<br>A* H cost: " +  n.getHCost() +
								"<br>A* F score: " + n.getFScore());
			usedCandidates.add(point);
		}

		Deque<DescriptedPoint> unUsedCandidates = new ArrayDeque<DescriptedPoint>();
		for (Node n : open) {
			DescriptedPoint point = n.getLocation();
			point.setDescription("<html>Visited but not selected cell" +
								 "<br>A* G cost: " +  n.getGCost() +
								 "<br>A* H cost: " +  n.getHCost() +
								 "<br>A* F score: " + n.getFScore());
			unUsedCandidates.add(point);
		}

		searchResult = new SearchResult(path, usedCandidates, unUsedCandidates,
				searchTime);

		return searchResult;
	}

	/**
	 * Constructs the found path.
	 * 
	 * @param node
	 *            Node object that is part of the found path.
	 * @param path
	 *            Deque<Point> where the path is stored
	 * @param closed
	 *            HashMap<Point, Node> of the closed points (see some A*
	 *            tutorial for more information)
	 */
	private void constructPath(Node node, Deque<DescriptedPoint> path,
			HashMap<DescriptedPoint, Node> closed) {

		if (node.getParent() != null) {
			path.push(node.getLocation());
			closed.remove(node.getLocation());
			constructPath(node.getParent(), path, closed);
		}

	}

	/**
	 * Searches path between given start and goal point using Map. Adds map
	 * points to PriorityQueue<Node> open and HashMap<Point, Node> closed (see
	 * some A* tutorial for more information) and the found path to Deque<Point>
	 * path.
	 * 
	 * 
	 * @param start
	 *            the point where the search begins
	 * @param goal
	 *            the goal point
	 * @param map
	 *            Map object the path finding algorithm uses
	 * @param open
	 *            PriorityQueue<Node> to which candidates for path are stored
	 * @param closed
	 *            HashMap<Point, Node> to where best candidates from open are
	 *            stored
	 * @param path
	 *            Deque<Point> to where the found path is stored
	 * @return true, if path was found and false if not
	 */
	private boolean doSearch(DescriptedPoint start, DescriptedPoint goal, Map map,
			PriorityQueue<Node> open, HashMap<DescriptedPoint, Node> closed,
			Deque<DescriptedPoint> path) {

		Node startNode = new Node(start, null);
		Node goalNode = new Node(goal, null);

		startNode.setGCost(0);
		startNode.setHCost(this.aStarCost.calculateHCost(startNode, goalNode));
		open.add(startNode);

		while (!open.isEmpty()) {

			Node current = open.poll();

			if (current == null) {
				break;
			}
			if (current.equals(goalNode)) {
				constructPath(current, path, closed);
				break;
			}

			closed.put(current.getLocation(), current);

			addNeighbours(current, goalNode, open, closed, map);

		}

		if (!path.isEmpty())
			return true;

		return false;

	}

	/**
	 * Checks neighbours for give Node and adds the suitable ones to
	 * PriorityQueue<Node> open.
	 * 
	 * @param current
	 *            Node to which neighbours are checked
	 * @param goal
	 *            the goal point
	 * @param open
	 *            PriorityQueue<Node> to which candidates for path are stored
	 * @param closed
	 *            HashMap<Point, Node> to where best candidates from open are
	 *            stored
	 * @param map
	 *            Map object the path finding algorithm uses
	 */
	private void addNeighbours(Node current, Node goal,
			PriorityQueue<Node> open, HashMap<DescriptedPoint, Node> closed, Map map) {

		if (current == null || goal == null || open == null || closed == null
				|| map == null)
			return;

		int column = current.getLocation().x;
		int row = current.getLocation().y;

		int rowLimit = map.getRows()-1;
		if (rowLimit > 0) {

			int colLimit = map.getColumns()-1;

			for (int nRow = Math.max(0, row - 1); nRow <= Math.min(row + 1,
					rowLimit); nRow++) {
				for (int nColumn = Math.max(0, column - 1); nColumn <= Math
						.min(column + 1, colLimit); nColumn++) {
					if (isNeighbourSuitable(column, row, nColumn, nRow, map))
						calculateNode(new DescriptedPoint(nColumn, nRow), current, goal,
								open, closed, map);
				}
			}

		}

	}

	/**
	 * Checks if point as a neighbour is suitable.
	 * 
	 * @param column
	 *            starting point's column index in map
	 * @param row
	 *            starting point's row index in map
	 * @param neighbourColumn
	 *            neighbour's column index
	 * @param neighbourRow
	 *            neighbour's row index
	 * @param map
	 *            Map object the path finding algorithm uses
	 * @return true, if neighbour point is suitable, false if not.
	 */
	private boolean isNeighbourSuitable(int column, int row,
			int neighbourColumn, int neighbourRow, Map map) {

		if ((column != neighbourColumn || row != neighbourRow)
				&& map.getMapCell(neighbourRow, neighbourColumn).isWalkable()) {

			if (column == neighbourColumn || row == neighbourRow)
				return true;

			int checkColumn = column;
			int checkRow = row;
			if ((column + 1) == neighbourColumn && (row + 1) == neighbourRow) {
				checkColumn += 1;
				checkRow += 1;
			} else if ((column - 1) == neighbourColumn
					&& (row - 1) == neighbourRow) {
				checkColumn -= 1;
				checkRow -= 1;
			} else if ((column + 1) == neighbourColumn
					&& (row - 1) == neighbourRow) {
				checkColumn += 1;
				checkRow -= 1;
			} else if ((column - 1) == neighbourColumn
					&& (row + 1) == neighbourRow) {
				checkColumn -= 1;
				checkRow += 1;
			}

			if (map.getMapCell(checkRow, column).isWalkable()
					&& map.getMapCell(row, checkColumn).isWalkable())
				return true;

			// if (MapCell.isWalkable(map[checkRow][column]) &&
			// MapCell.isWalkable(map[row][checkColumn]))
			// return true;

		}

		return false;
	}

	/**
	 * Creates and calculates node.
	 * 
	 * @param point
	 *            for which the Node object is created and calculated
	 * @param current
	 *            Node which was handled and to which neighbours checked
	 * @param goal
	 *            the goal point
	 * @param open
	 *            PriorityQueue<Node> to which candidates for path are stored
	 * @param closed
	 *            HashMap<Point, Node> to where best candidates from open are
	 *            stored
	 * @param map
	 *            Map object the path finding algorithm uses
	 */
	private void calculateNode(DescriptedPoint point, Node current, Node goal,
			PriorityQueue<Node> open, HashMap<DescriptedPoint, Node> closed, Map map) {

		if (closed.containsKey(point))
			return;

		Node neighbour = new Node(point, current);
		Node node = null;

		// check if neighbour node is in open nodes and if it is, get it
		Iterator<Node> iter = open.iterator();
		while (iter.hasNext()) {
			Node n = iter.next();
			if (n.equals(neighbour)) {
				node = n;
				break;
			}
		}

		if (node != null) {

			// check if current node would be better parent for node
			int gCostWithNewParent = this.aStarCost.calculateGCost(node,
					current, map);

			if (gCostWithNewParent < node.getGCost()) {
				node.setParent(current);
				node.setGCost(gCostWithNewParent);
				node.setHCost(this.aStarCost.calculateHCost(node, goal));
			}

		} else { // neighbour node not in open list => add it
			neighbour.setParent(current);
			neighbour.setGCost(this.aStarCost.calculateGCost(neighbour,
					current, map));
			neighbour.setHCost(this.aStarCost.calculateHCost(neighbour, goal));

			open.add(neighbour);
		}

	}

	@Override
	public String toString() {
		return "A* algorithm";
	}

}
